#include <Arduino.h>
#include "StepMotor.h"
#include"motorDrive.h"
#include"mb_pins.h"
#include "PinMap.h"
// #include "FashionStar_UartServoProtocol.h"
// #include "FashionStar_UartServo.h" 
//步进电机1 脉冲 方向
#define PUL_1 8
#define DIR_1 10
//步进电机2 脉冲 方向
#define PUL_2 5
#define DIR_2 6
//步进电机3 脉冲 方向
#define PUL_3 1
#define DIR_3 2
//步进电机4 脉冲 方向
#define PUL_4 3
#define DIR_4 4

#define LED2 15

#define OLED_FPS 60          // OLED屏幕刷新率

#include "mpu6050.hpp"

#include <U8g2lib.h>
#include "oledDisplay.h"
U8G2_SSD1306_128X64_NONAME_F_HW_I2C U8G2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ I2C_SCL, /* data=*/ I2C_SDA);
StepMotor MOTOR_0(PUL_3, DIR_3, 0);
StepMotor MOTOR_1(PUL_4, DIR_4, 1);



void task_blink(void * ptr){
    pinMode(LED2, OUTPUT);
    while (true){
        vTaskDelay(500);
        digitalWrite(LED2, HIGH);
        vTaskDelay(500);
        digitalWrite(LED2, LOW);
    }  
}

#define angle1 26
#define angle2 332

void reqHandler(String req){
    bool valid = false;
    if (req.substring(0, 1) == ":"){
        uint8_t motorId = req.substring(1).toInt();
        double spd = req.substring(3).toFloat();
        if (motorId == 0){
            MOTOR_0.motorSpeed(spd);
            Serial.println("stepmotor parameter:");
            Serial.printf("motorID:%d   speed:%.1f  \n",motorId,spd);
            valid = true;
        }
        if (motorId == 1){
            MOTOR_1.motorSpeed(spd);
            Serial.println("stepmotor parameter:");
            Serial.printf("motorID:%d   speed:%.1f  \n",motorId,spd);
            valid = true;
        }
        if (motorId == 4){
            Serial.println("X归零");
            ledcSetup(4, 50, 10); // 设置通道0的频率为50Hz，分辨率为10位
            ledcAttachPin(SERVO_PIN_X, 4); 
            ledcWrite(4, angle1);   //——36
            vTaskDelay(500);
            valid = true;
        }
        if (motorId == 5){
            ledcSetup(4, 50, 10); // 设置通道0的频率为50Hz，分辨率为10位
            ledcAttachPin(SERVO_PIN_X, 4);
            Serial.println("X旋转90°");
            ledcWrite(4, angle2); // 90°  55 -45
            vTaskDelay(500);
            valid = true;
        }
        if (motorId == 6){
            ledcSetup(5, 50, 10); // 设置通道1的频率为50Hz，分辨率为10位
            ledcAttachPin(SERVO_PIN_X1, 5); // 将通道0与引脚3连接起来
            Serial.println("X1归零");
            ledcWrite(5, angle1); // 0°
            vTaskDelay(500);
            valid = true;

        }
        if (motorId == 7){
            ledcSetup(5, 50, 10); // 设置通道1的频率为50Hz，分辨率为10位
            ledcAttachPin(SERVO_PIN_X1, 5); // 将通道0与引脚3连接起来
            Serial.println("X1旋转90°");
            ledcWrite(5, angle2); // 90°
            vTaskDelay(500);
            valid = true;
        }
        if (motorId == 8){
            Serial.println("Y归零");
            ledcSetup(6, 50, 10); // 设置通道2的频率为50Hz，分辨率为10位
            ledcAttachPin(SERVO_PIN_Y, 6); // 将通道0与引脚18连接起来
            ledcWrite(6, angle2);// 180°
            vTaskDelay(500);
            valid = true;
        }
        if (motorId == 9){
            Serial.println("Y旋转90°");
            ledcSetup(6, 50, 10); // 设置通道2的频率为50Hz，分辨率为10位
            ledcAttachPin(SERVO_PIN_Y, 6); // 将通道0与引脚18连接起来
            ledcWrite(6, angle1); // 0°
            vTaskDelay(500);
            valid = true;
        }
        // vTaskDelay(1000);
        ledcDetachPin(SERVO_PIN_X); // 断开引脚与通道的连接
        ledcDetachPin(SERVO_PIN_X1); // 将通道0与引脚3连接起来
        ledcDetachPin(SERVO_PIN_Y); // 将通道0与引脚3连接起来
    }   
// 舵机指令       
}

void task_req_analyze(void * ptr){
    String req;
    while (true){
        req = "";
        if (Serial.available()){
            req = Serial.readStringUntil('\n');
            reqHandler(req);
        }
    vTaskDelay(10);
    }
}

// PID参数
float Kp = 100, Ki = 1, Kd = 0;
float integral = 0, previous_error = 0;
unsigned long last_time = 0;

void print_mpu6050_datas(){
    static MPU6050Data_t data;
    update_mpu6050_data(&data);
    Serial.print(1000*data.accelerometer_x); Serial.print(", ");
    Serial.print(1000*data.accelerometer_y); Serial.print(", ");
    Serial.print(1000*data.accelerometer_z); Serial.print(", ");
    Serial.print(data.gyroscope_x, 1); Serial.print(", ");
    Serial.print(data.gyroscope_y, 1); Serial.print(", ");
    Serial.print(data.gyroscope_z, 1);Serial.print("\n");
    int pitch = static_cast<int>(atan2(data.accelerometer_x,sqrt(data.accelerometer_y  * data.accelerometer_y  + data.accelerometer_z  * data.accelerometer_z))  * 180.0 / PI); 
    Serial.print("Pitch  angle: "); 
    Serial.print(pitch); 
    Serial.println("  degrees"); 
    Serial.println("");

    float setpoint = 3.0; // 目标pitch值，即期望的pitch值
    float error = setpoint - pitch; // 计算当前pitch与目标pitch之间的误差
    unsigned long current_time = millis(); // 获取当前时间（毫秒）
    float delta_time = (current_time - last_time) / 1000.0; // 计算时间间隔（秒）
    
    integral += error * delta_time; // 累积误差，用于积分项
    float derivative = (error - previous_error) / delta_time; // 计算误差的变化率，用于微分项
    float output = Kp * error + Ki * integral + Kd * derivative; // 根据PID公式计算输出值
      // 限制输出范围（假设电机最大响应为±1000步）
    output = constrain(output, -1000, 1000);
    
    MOTOR_0.motorSpeed(output); // 设置电机的速度为计算出的输出值
    
    previous_error = error; // 更新上一次的误差值
    last_time = current_time; // 更新上一次的时间

    // 调试输出
    Serial.print(" error: ");
    Serial.println(error);
    Serial.print("°, output: ");
    Serial.println(output);
}


// void task_u8g2(void * ptr){
//     U8G2.begin();  // 初始化显示屏
//     for(;;){
//         U8G2.clearBuffer(); // 清除缓冲区
//         U8G2.setFont(u8g2_font_ncenB08_tr); // 设置字体
//         U8G2.setCursor(0, 10); // 设置起始位置
//         U8G2.print("Motor Speed:"); // 打印标签
//         U8G2.setCursor(0, 24); // 设置起始位置
//         U8G2.printf(" 0 :  %.1f", MOTOR_0.freqSpeed); // 打印标签
//         U8G2.setCursor(0, 38); // 设置起始位置
//         U8G2.printf(" 1 :  %.1f", MOTOR_1.freqSpeed); // 打印标签
//         U8G2.setCursor(0, 52); // 设置起始位置
//         U8G2.printf(" 2 :  %.1f", MOTOR_0.freqSpeed); // 打印标签
//         U8G2.sendBuffer();
//         vTaskDelay(50);
//     }
// }

void setup(){
    Serial.begin(115200);
    pinMode(12,OUTPUT);
    pinMode(LED2,OUTPUT);
    digitalWrite(12,LOW);
    mpu6050_setup();
    xTaskCreate(task_blink, "ONBOARD_LED_BLINK", 1024, NULL, 1, NULL); 
    xTaskCreate(task_req_analyze, "REQ_ANALYZE",     1024 * 6, NULL, 2, NULL);
    // xTaskCreate(task_u8g2, "OLED_DISPLAY",     1024 * 6, NULL, 2, NULL);
}

void loop(){
    print_mpu6050_datas();
    vTaskDelay(1000);
}